package com.handinfo.util;

/**
 * <p>
 * Title: 二叉堆A*寻路算法
 * </p>
 * 
 * <p>
 * Description: 使用最小二叉堆实现A*寻路算法
 * 
 * A*算法实现方法： 1.把起点添加到开启列表。 2.重复如下的工作： a) 寻找开启列表中F值最低的节点。我们称它为当前节点。 b) 把它切换到关闭列表。 c) 对相邻的4格中的每一个？ *
 * 如果它不可通过或者已经在关闭列表中，略过它。反之如下。 * 如果它不在开启列表中，把它添加进去。把当前格作为这一节点的父节点。记录这一格的F,G,和H值。 *
 * 如果它已经在开启列表中，用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径
 * 。如果是这样，就把这一节点的父节点改成当前节点，并且重新计算这一节点的G和F值。如果你保持你的开启列表按F值排序，改变之后你可能需要重新对开启列表排序。 d) 停止，当你 * 把终点添加进了关闭列表(注解)，这时候路径被找到 *
 * 没有找到终点，开启列表已经空了。这时候，路径不存在。 3.保存路径。从终点开始，沿着每一节点的父节点移动直到回到起点。这就是你的路径。
 * 
 * 二叉堆概念： 以最小二叉堆为例 最小二叉堆是一组元素，其中最小的元素在堆顶端。
 * 
 * 具体实现时，数据结构可以为一个一维数组： 1.其首元素在索引1位置上。（索引0位置用任意填充） 2.任何元素的两个子节点可以通过把当前元素的位置乘以2（得到第一个子节点）和乘2加1（得到第二个子节点）来得到。
 * 3.任意父元素比其子元素小
 * 
 * 最小二叉堆的添加，删除操作 添加：将元素添加到数组尾部，然后和其父节点比较大小（可以通过 索引/2 得到 其父节点的索引）， 如果小，则与父节点交换位置，重复这个过程直到该元素不再比它的父节点小，或者该元素到达数组顶端（即索引为1）
 * 删除：二叉堆的删除操作一般针对顶端元素。具体操作如下：用数组尾部元素替换到顶端元素，然后，将该元素不断与 自己的子节点比较大小，如果比其中任意一个子节点大的时候，则与较小的子节点交换位置，重复这个过程，直到该元素
 * 不再比它的任意子节点大，或者到达数组尾部。
 * </p>
 * 
 * @author Xia Mingwei
 * @version 1.0
 */
public class BinHeapAStar
{
	// 各节点的内部结构索引
	private static final int NODE_ID = 0; // id
	private static final int F_VALUE = 1; // F值
	private static final int H_VALUE = 2; // H值
	private static final int G_VALUE = 3; // G值
	private static final int PARENT_ID = 4; // 父节点id

	public static final int SIZE = 4;

	private static int s_start, s_dest; // 起点和终点
	private static byte[] s_maps; // 地图数据
	private static int s_map_width; // 地图宽度
//	private static int s_map_height; // 地图高度
	private static short[][] s_open_list; // openlist
	private static int s_open_list_count = 1; // openlist的节点个数（注意：为了符合二叉堆的结构，节点从索引1开始计数）
	private static short[][] s_close_list; // closelist
	private static int s_close_list_count = 0; // closelist的节点个数
	private static short[] s_cur_node = new short[5]; // 当前节点
	private static boolean s_dest_in_closelist; // 目标节点是否已经在closelist中（循环终结条件之一）

	/**
	 * 初始化相关属性
	 * 
	 * @param map
	 * byte[]
	 * @param map_w
	 * int
	 * @param map_h
	 * int
	 * @param start
	 * int
	 * @param dest
	 * int
	 */
	private static void init(byte[] map, int map_w, int map_h, int start, int dest)
	{
		s_maps = null;
		s_maps = map;
//		s_map_height = map_h;
		s_map_width = map_w;
		s_start = start;
		s_dest = dest;
		s_open_list_count = 1;
		s_close_list_count = 0;
		if (s_open_list == null) {
			s_open_list = new short[SIZE][5];
		}
		if (s_close_list == null) {
			s_close_list = new short[SIZE][5];
		}
		s_dest_in_closelist = false;
		addToOpenList(
				s_start,
				Math.abs(s_dest / s_map_width - s_start / s_map_width)
						+ Math.abs(s_dest % s_map_width - s_start % s_map_width),
				0, -1);
	}

	/**
	 * 添加节点到openlist
	 * 
	 * @param id
	 * int
	 * @param h_value
	 * int
	 * @param g_value
	 * int
	 * @param parent_id
	 * int
	 */
	private static void addToOpenList(int id, int h_value, int g_value, int parent_id)
	{
		resizeOpenlist();
		int inx = s_open_list_count;
		short[] tmp = null;
		s_open_list[inx][NODE_ID] = (short) id;
		s_open_list[inx][H_VALUE] = (short) h_value;
		s_open_list[inx][G_VALUE] = (short) g_value;
		s_open_list[inx][F_VALUE] = (short) (h_value + g_value);
		s_open_list[inx][PARENT_ID] = (short) parent_id;
		s_open_list_count++;
		while (inx > 1) {
			if (s_open_list[inx][F_VALUE] <= s_open_list[inx / 2][F_VALUE]) {
				tmp = s_open_list[inx];
				s_open_list[inx] = s_open_list[inx / 2];
				s_open_list[inx / 2] = tmp;
				inx /= 2;
			}
			else {
				break;
			}
		}
	}

	/**
	 * 从openlist中移除节点（该节点为openlist中的第一个节点，其F值最小）
	 * 具体做法为：用最后一个节点替换第一个节点，然后将它不断的跟其子节点（这里的子节点索引符合二叉堆结构）比较F值
	 * 如果F值比子节点大，则跟较小的子节点交换位置 如果F值小于任意子节点，则结束比较
	 */
	private static void removeFromOpenList()
	{
		s_open_list[1][NODE_ID] = s_open_list[s_open_list_count - 1][NODE_ID];
		s_open_list[1][H_VALUE] = s_open_list[s_open_list_count - 1][H_VALUE];
		s_open_list[1][G_VALUE] = s_open_list[s_open_list_count - 1][G_VALUE];
		s_open_list[1][F_VALUE] = s_open_list[s_open_list_count - 1][F_VALUE];
		s_open_list[1][PARENT_ID] = s_open_list[s_open_list_count - 1][PARENT_ID];
		s_open_list_count--;
		int inx = 1;
		short[] tmp = null;
		while (true) {
			// 两个子节点均存在
			if (inx * 2 + 1 < s_open_list_count) {
				int nextInx = inx;
				if (s_open_list[inx][F_VALUE] >= s_open_list[inx * 2][F_VALUE]) {
					nextInx = inx * 2;
				}
				if (s_open_list[nextInx][F_VALUE] >= s_open_list[inx * 2 + 1][F_VALUE]) {
					nextInx = inx * 2 + 1;
				}
				if (nextInx != inx) {
					tmp = s_open_list[inx];
					s_open_list[inx] = s_open_list[nextInx];
					s_open_list[nextInx] = tmp;
					inx = nextInx;
				}
				else {
					break;
				}
			}
			// 只存在一个子节点
			else if (inx * 2 < s_open_list_count) {
				if (s_open_list[inx][F_VALUE] >= s_open_list[inx * 2][F_VALUE]) {
					tmp = s_open_list[inx];
					s_open_list[inx] = s_open_list[inx * 2];
					s_open_list[inx * 2] = tmp;
					inx = inx * 2;
				}
				else {
					break;
				}
			}
			else {
				break;
			}
		}
	}

	/**
	 * 重新调整openlist的大小
	 */
	private static void resizeOpenlist()
	{
		if (s_open_list_count >= s_open_list.length) {
			short[][] temp = new short[s_open_list.length << 1][5];
			System.arraycopy(s_open_list, 0, temp, 0, s_open_list.length);
			s_open_list = temp;
			temp = null;
		}
	}

	/**
	 * 添加节点到closelist
	 * 
	 * @param node
	 * short[]
	 */
	private static void addToCloseList(short[] node)
	{
		resizeCloselist();
		s_close_list[s_close_list_count][NODE_ID] = node[NODE_ID];
		s_close_list[s_close_list_count][F_VALUE] = node[F_VALUE];
		s_close_list[s_close_list_count][G_VALUE] = node[G_VALUE];
		s_close_list[s_close_list_count][H_VALUE] = node[H_VALUE];
		s_close_list[s_close_list_count][PARENT_ID] = node[PARENT_ID];
		s_close_list_count++;
	}

	/**
	 * 重新调整closelist的大小
	 */
	private static void resizeCloselist()
	{
		if (s_close_list_count >= s_close_list.length) {
			short[][] temp = new short[s_close_list.length << 1][5];
			System.arraycopy(s_close_list, 0, temp, 0, s_close_list.length);
			s_close_list = temp;
			temp = null;
		}
	}

	/**
	 * closelist中是否包含某节点
	 * 
	 * @param id
	 * int
	 * @return boolean
	 */
	private static boolean isCloseListContainNode(int id)
	{
		for (int i = 0; i < s_close_list_count; i++) {
			if (s_close_list[i][NODE_ID] == id)
				return true;
		}
		return false;
	}

	/**
	 * 设置当前节点（openlist中F值最小的节点，即第一个节点）
	 * 
	 * @param node
	 * short[]
	 */
	private static void setCurNode(short[] node)
	{
		// openlist中第一个节点的F值最小
		node[NODE_ID] = s_open_list[1][NODE_ID];
		node[G_VALUE] = s_open_list[1][G_VALUE];
		node[H_VALUE] = s_open_list[1][H_VALUE];
		node[F_VALUE] = s_open_list[1][F_VALUE];
		node[PARENT_ID] = s_open_list[1][PARENT_ID];
	}

	/**
	 * 分析周边节点
	 * 
	 * @param node
	 * short[]
	 */
	private static void analyseAround(short[] node)
	{
		// 尽量使路径保证直线
		if (node[NODE_ID] - node[PARENT_ID] == 1) {
			// up
			analyseNode(node[NODE_ID] - s_map_width, 1);
			// down
			analyseNode(node[NODE_ID] + s_map_width, 1);
			if (node[NODE_ID] % s_map_width != 0) { // 向左查找换行了
				// left
				analyseNode(node[NODE_ID] - 1, 1);
			}
			if (node[NODE_ID] % s_map_width != s_map_width - 1) { // 向右查找换行了
				// right
				analyseNode(node[NODE_ID] + 1, 1);
			}
		}
		else if (node[NODE_ID] - node[PARENT_ID] == -1) {
			// up
			analyseNode(node[NODE_ID] - s_map_width, 1);
			// down
			analyseNode(node[NODE_ID] + s_map_width, 1);
			if (node[NODE_ID] % s_map_width != s_map_width - 1) { // 向右查找换行了
				// right
				analyseNode(node[NODE_ID] + 1, 1);
			}
			if (node[NODE_ID] % s_map_width != 0) { // 向左查找换行了
				// left
				analyseNode(node[NODE_ID] - 1, 1);
			}
		}
		else if (node[NODE_ID] - node[PARENT_ID] == s_map_width) {
			// up
			analyseNode(node[NODE_ID] - s_map_width, 1);
			if (node[NODE_ID] % s_map_width != s_map_width - 1) { // 向右查找换行了
				// right
				analyseNode(node[NODE_ID] + 1, 1);
			}
			if (node[NODE_ID] % s_map_width != 0) { // 向左查找换行了
				// left
				analyseNode(node[NODE_ID] - 1, 1);
			}
			// down
			analyseNode(node[NODE_ID] + s_map_width, 1);
		}
		else if (node[NODE_ID] - node[PARENT_ID] == -s_map_width) {
			// down
			analyseNode(node[NODE_ID] + s_map_width, 1);
			if (node[NODE_ID] % s_map_width != s_map_width - 1) { // 向右查找换行了
				// right
				analyseNode(node[NODE_ID] + 1, 1);
			}
			if (node[NODE_ID] % s_map_width != 0) { // 向左查找换行了
				// left
				analyseNode(node[NODE_ID] - 1, 1);
			}
			// up
			analyseNode(node[NODE_ID] - s_map_width, 1);
		}
		else {
			// up
			analyseNode(node[NODE_ID] - s_map_width, 1);
			// down
			analyseNode(node[NODE_ID] + s_map_width, 1);
			if (node[NODE_ID] % s_map_width != 0) { // 向左查找换行了
				// left
				analyseNode(node[NODE_ID] - 1, 1);
			}
			if (node[NODE_ID] % s_map_width != s_map_width - 1) { // 向右查找换行了
				// right
				analyseNode(node[NODE_ID] + 1, 1);
			}
		}
	}

	/**
	 * 分析特定节点 具体操作如下： 1.id超过map范围，则返回 2.该节点在closelise中或者不可通过，则返回
	 * 3.该节点不在openlist中则添加到openlist中
	 * 4.该节点在openlist中，如果新路径G值比原路径G值小，则使用新路径，并且对openlist进行重新排序；
	 * 如果新路径G值比原路径G值大，不进行任何改变
	 * 
	 * @param id
	 * int
	 * @param g_value
	 * int 从当前节点到特定节点的G值
	 */
	private static void analyseNode(int id, int g_value)
	{
		if (id < 0 || id >= s_maps.length)
			return;
		if (isCloseListContainNode(id) || isBlocked(s_maps[id]))
			return;

		boolean contain = false;
		for (int i = 1; i < s_open_list_count; i++) {
			if (s_open_list[i][NODE_ID] == id) {
				contain = true;
				if (s_cur_node[G_VALUE] + g_value < s_open_list[i][G_VALUE]) {
					s_open_list[i][G_VALUE] = (short) (s_cur_node[G_VALUE] + g_value);
					s_open_list[i][F_VALUE] = (short) (s_open_list[i][G_VALUE] + s_open_list[i][H_VALUE]);
					s_open_list[i][PARENT_ID] = s_cur_node[NODE_ID];
					// F值有变化，所以二叉堆需要进行重新的排序
					int inx = i;
					short[] tmp = null;
					while (inx > 1) {
						if (s_open_list[inx][F_VALUE] <= s_open_list[inx / 2][F_VALUE]) {
							tmp = s_open_list[inx];
							s_open_list[inx] = s_open_list[inx / 2];
							s_open_list[inx / 2] = tmp;
							inx /= 2;
						}
						else {
							break;
						}
					}
				}
				break;
			}
		}
		if (!contain) {
			int g = s_cur_node[G_VALUE] + g_value;
			int h = Math.abs(s_dest / s_map_width - id / s_map_width)
					+ Math.abs(s_dest % s_map_width - id % s_map_width);
			int parent_id = s_cur_node[NODE_ID];
			addToOpenList(id, h, g, parent_id);
		}
	}

	/**
	 * 根据closelist中的节点信息，从终点开始查找父节点，直到找到起点，并且创建最终的路径数组
	 * 
	 * @param close_list
	 * short[][]
	 * @param close_list_count
	 * int
	 * @return short[]
	 */
	private static short[] getPath(short[][] close_list, int close_list_count)
	{
		short[] tmp = new short[close_list_count];
		int tmp_count = 0;
		short[] cursor_node = close_list[close_list_count - 1];
		while (cursor_node[NODE_ID] != s_start) {
			tmp[tmp_count++] = cursor_node[NODE_ID];
			for (int i = 0; i < close_list_count; i++) {
				if (close_list[i][NODE_ID] == cursor_node[PARENT_ID]) {
					cursor_node = close_list[i];
					break;
				}
			}
		}
		tmp[tmp_count++] = (short) s_start;
		short[] path = new short[tmp_count];
		for (int i = 0; i < tmp_count; i++) {
			path[i] = tmp[tmp_count - 1 - i];
		}
		// System.arraycopy(tmp, 0, path, 0, tmp_count);
		return path;
	}

	/**
	 * 寻路主方法 寻路算法的入口，外部程序直接调用该方法获得寻路结果
	 * 
	 * @param map
	 * byte[]
	 * @param map_w
	 * int
	 * @param map_h
	 * int
	 * @param start
	 * int
	 * @param dest
	 * int
	 * @return short[]
	 */
	public static short[] findPath(byte[] map, int map_w, int map_h, int start, int dest)
	{
		if (map == null || dest >= map.length || isBlocked(map[dest])) {
			return null;
		}
		try {
			init(map, map_w, map_h, start, dest);
			while (!s_dest_in_closelist && s_open_list_count > 1) {
				setCurNode(s_cur_node);
				removeFromOpenList();
				addToCloseList(s_cur_node);
				if (s_cur_node[NODE_ID] == s_dest) {
					s_dest_in_closelist = true;
				}
				else {
					analyseAround(s_cur_node);
				}
			}
			return getPath(s_close_list, s_close_list_count);
		}
		catch (Exception ex) {
			Tools.debug(ex);
			return null;
		}
	}
	/**
	 * 将x,y转换为点
	 * @param x
	 * @param y
	 * @param w 单元块宽
	 * @param h 单元块高
	 * @param cols 横向格数
	 * @return
	 */
	public static final int convertXYtoPoint(int x, int y, int w, int h, int cols) {
		return (y / h * cols + x / w);
	}

	public static boolean isBlocked(int value)
	{
		return value == 0;
	}
}
